#include "Viewer.h"
#include <pangolin/pangolin.h>

#include <mutex>

namespace msf
{

Viewer::Viewer(System* pSystem, MapDrawer *pMapDrawer, Tracking *pTracking, Settings* settings):
    both(false), mpSystem(pSystem),mpMapDrawer(pMapDrawer), mpTracker(pTracking),
    mbFinishRequested(false), mbFinished(true), mbStopped(true), mbStopRequested(false)
{
    if(settings)
    {
        newParameterLoader(settings);
    }
    else
    {
        std::cerr << "**ERROR in the config file**" << std::endl;
        try
        {
            throw -1;
        }
        catch(exception &e)
        {

        }
    }

    mbStopTrack = false;
}

void Viewer::newParameterLoader(Settings *settings) 
{

    mViewpointX = settings->viewPointX();
    mViewpointY = settings->viewPointY();
    mViewpointZ = settings->viewPointZ();
    mViewpointF = settings->viewPointF();

}

void Viewer::Run()
{
    mbFinished = false;
    mbStopped = false;

    pangolin::CreateWindowAndBind("Multi-Sensor Fusion: Track Viewer",1024,768);

    // 3D Mouse handler requires depth testing to be enabled
    glEnable(GL_DEPTH_TEST);

    // Issue specific OpenGl we might need
    glEnable (GL_BLEND);
    glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    // 在屏幕顶部创建名为"menu"的控制面板，占据屏幕高度的1/175
    pangolin::CreatePanel("menu").SetBounds(0.0,1.0,0.0,pangolin::Attach::Pix(175));
    // 创建布尔类型的控制变量，分别表示是否跟随里程计，是否显示里程计视图，是否显示俯视图
    pangolin::Var<bool> menuFollowOdom("menu.Follow Odom",false,true);
    pangolin::Var<bool> menuOdomView("menu.Odom View",false,false);
    pangolin::Var<bool> menuTopView("menu.Top View",false,false);
    // 创建布尔类型的控制变量，分别表示是否显示关键帧、是否显示惯性图、是否重置、是否停止
    pangolin::Var<bool> menuShowKeyPoseFrames("menu.Show KeyFrames",true,true);
    pangolin::Var<bool> menuShowInertialGraph("menu.Show Inertial Graph",true,true);
    pangolin::Var<bool> menuLocalizationMode("menu.Localization Mode",false,true);
    pangolin::Var<bool> menuReset("menu.Reset",false,false);
    pangolin::Var<bool> menuStop("menu.Stop",false,false);

    // 定义相机渲染对象(用于视图/场景浏览)
    pangolin::OpenGlRenderState s_cam(
                // 设置投影矩阵，参数分别是宽度，高度，视场角，纵横比，近平面，远平面
                pangolin::ProjectionMatrix(1024,768,mViewpointF,mViewpointF,512,389,0.1,1000),
                // 设置模型视图矩阵，参数分别是相机位置，目标位置，上方向
                pangolin::ModelViewLookAt(mViewpointX,mViewpointY,mViewpointZ, 0,0,0,0.0,-1.0, 0.0)
                );
    
    // 将命名的OpenGL视点添加到窗口并提供3D处理程序
    pangolin::View& d_cam = pangolin::CreateDisplay()
            // 设置视点的边界，参数分别是左下角和右上角的比例或像素值，以及纵横比
            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f/768.0f)
            // 设置视点的处理器，用于响应鼠标和键盘事件
            .SetHandler(new pangolin::Handler3D(s_cam));

    pangolin::OpenGlMatrix Two;
    Two.SetIdentity();
    pangolin::OpenGlMatrix Ow; // Oriented with g in the z axis
    Ow.SetIdentity();

    bool bFollow = true;
    bool bOdomView = true;
    bool bLocalizationMode = false;

    cout << "Viewer THREAD: Starting the Viewer" << endl;
    while(1)
    {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        mpMapDrawer->GetCurrentOpenGLOdomMatrix(Two,Ow);

        if(mbStopTrack)
        {
            //menuStepByStep = true;
            mbStopTrack = false;
        }

        if(menuFollowOdom && bFollow)
        {
            if(bOdomView)
                s_cam.Follow(Two);
            else
                s_cam.Follow(Ow);
        }
        else if(menuFollowOdom && !bFollow)
        {
            if(bOdomView)
            {
                s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,mViewpointF,mViewpointF,512,389,0.1,1000));
                s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(mViewpointX,mViewpointY,mViewpointZ, 0,0,0,0.0,-1.0, 0.0));
                s_cam.Follow(Two);
            }
            else
            {
                s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,3000,3000,512,389,0.1,1000));
                s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(0,0.01,10, 0,0,0,0.0,0.0, 1.0));
                s_cam.Follow(Ow);
            }
            bFollow = true;
        }
        else if(!menuFollowOdom && bFollow)
        {
            bFollow = false;
        }

        if(menuOdomView)
        {
            menuOdomView = false;
            bOdomView = true;
            s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,mViewpointF,mViewpointF,512,389,0.1,10000));
            s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(mViewpointX,mViewpointY,mViewpointZ, 0,0,0,0.0,-1.0, 0.0));
            s_cam.Follow(Two);
        }

        if(menuTopView && mpMapDrawer->mpAtlas->isImuInitialized())
        {
            menuTopView = false;
            bOdomView = false;
            s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,3000,3000,512,389,0.1,10000));
            s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(0,0.01,50, 0,0,0,0.0,0.0, 1.0));
            s_cam.Follow(Ow);
        }

        if(menuLocalizationMode && !bLocalizationMode)
        {
            mpSystem->ActivateLocalizationMode();
            bLocalizationMode = true;
        }
        else if(!menuLocalizationMode && bLocalizationMode)
        {
            mpSystem->DeactivateLocalizationMode();
            bLocalizationMode = false;
        }        

        d_cam.Activate(s_cam);
        glClearColor(1.0f,1.0f,1.0f,1.0f);
        mpMapDrawer->DrawCurrentOdom(Two);
        if(menuShowInertialGraph || menuShowKeyPoseFrames)
            mpMapDrawer->DrawKeyPoseFrames(menuShowKeyPoseFrames, menuShowInertialGraph);

        pangolin::FinishFrame();

        if(menuReset)
        {
            menuShowInertialGraph = true;
            menuShowKeyPoseFrames = true;
            bFollow = true;
            menuFollowOdom = true;
            mpSystem->ResetActiveMap();
            menuReset = false;
        }

        if(menuStop)
        {
            if(bLocalizationMode)
                mpSystem->DeactivateLocalizationMode();

            // 保存相机轨迹
            mpSystem->SaveTrajectoryEuRoC("CameraTrajectory.txt");
            // 保存关键帧轨迹
            mpSystem->SaveKeyPoseFrameTrajectoryEuRoC("KeyPoseFrameTrajectory.txt");

            // 停止所有线程
            mpSystem->Shutdown();

            // 重置菜单停止标志
            menuStop = false;
        }

        // 暂停
        // 检查系统是否停止
        if(Stop())
        {
            // 等待系统恢复
            while(isStopped())
            {
                usleep(3000);
            }
        }

        // 检查系统是否结束
        if(CheckFinish())
            break;

    }
    SetFinish();
}

// 外部请求停止
void Viewer::RequestFinish()
{
    unique_lock<mutex> lock(mMutexFinish);
    mbFinishRequested = true;
}

// 内部设置结束
void Viewer::SetFinish()
{
    unique_lock<mutex> lock(mMutexFinish);
    mbFinished = true;
}

// 内部判断是否结束
bool Viewer::CheckFinish()
{
    unique_lock<mutex> lock(mMutexFinish);
    return mbFinishRequested;
}

// 外部查看是否结束
bool Viewer::isFinished()
{
    unique_lock<mutex> lock(mMutexFinish);
    return mbFinished;
}

void Viewer::RequestStop()
{
    unique_lock<mutex> lock(mMutexStop);
    if(!mbStopped)
        mbStopRequested = true;
}

bool Viewer::isStopped()
{
    unique_lock<mutex> lock(mMutexStop);
    return mbStopped;
}

bool Viewer::Stop()
{
    unique_lock<mutex> lock(mMutexStop);
    unique_lock<mutex> lock2(mMutexFinish);

    if(mbFinishRequested)
        return false;
    else if(mbStopRequested)
    {
        mbStopped = true;
        mbStopRequested = false;
        return true;
    }
    return false;
}

void Viewer::Release()
{
    unique_lock<mutex> lock(mMutexStop);
    mbStopped = false;
}

}